#include <stddef.h>
#include <stdint.h>
#include "stm32wlxx.h"
#include "Legacy/stm32_hal_legacy.h"  /* Aliases file for old names compatibility */
#include "stm32wlxx_hal_def.h"
#include "stm32wlxx_hal_i2c.h"
#include "stm32wlxx_hal_gpio.h"
#include "stm32wlxx_hal_rcc.h"
#include "stm32wlxx_hal.h"
#include "stm32wle5xx.h"
#include "apps.h"



uint8_t tca6408a_addr = 0;   // Detected device address (shifted left)
uint8_t config;
uint8_t output_state = 0x00;

#define LDO_VDDL_EN_PIN GPIO_PIN_6
#define LDO_VDDL_EN_GPIO_Port GPIOA
static uint32_t _io_init_state = 0; // Initialize as a bitmask to track initialized pins

void io_init(void * _port, uint16_t pins){
    GPIO_TypeDef * port = (GPIO_TypeDef *)_port;
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Pin = pins;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_PULLDOWN;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(port, &GPIO_InitStruct);
    _io_init_state |= (1 << pins) << ((port == GPIOA) ? 0 : (port == GPIOB) ? 16 : 24); // Use bitwise OR to track multiple pins
}

void io_deinit(){
    HAL_GPIO_DeInit(LDO_VDDL_EN_GPIO_Port, LDO_VDDL_EN_PIN);
}

void ldo_vddl_ctrl(int enable){
    HAL_GPIO_WritePin(LDO_VDDL_EN_GPIO_Port, LDO_VDDL_EN_PIN, (enable?GPIO_PIN_SET:GPIO_PIN_RESET));
    if (!(_io_init_state & (1 << LDO_VDDL_EN_PIN))) { // Check if the pin is not already initialized
        io_init(LDO_VDDL_EN_GPIO_Port, LDO_VDDL_EN_PIN);
    }
}

void ldo_2v5_ctrl(int enable){
    ext_io_ctrl(LDO_2V5_EN, enable);
}

extern uint8_t tca6408a_addr;
extern uint8_t config;
extern uint8_t output_state ;

/* Private defines -----------------------------------------------------------*/
/*
输入端口寄存器(0x00)：只读，反映输入引脚的状态
输出端口寄存器(0x01)：读写，控制输出引脚的状态
极性反转寄存器(0x02)：读写，反转输入引脚的极性
配置寄存器(0x03)：读写，配置I/O方向(1=输入，0=输出)
*/
#define TCA6408A_CONFIG_REG    0x03   // Configuration register address
#define TCA6408A_POLARITY_REG  0x02   // Polarity register address
#define TCA6408A_OUTPUT_REG    0x01   // Output register address
#define TCA6408A_INPUT_REG     0x00   // Input register address
// TCA6408A possible 7-bit addresses range from 0x20 to 0x21
#define TCA6408A_ADDR_START    0x20
#define TCA6408A_ADDR_END      0x21


int ext_io_ctrl(enum EXT_IO_ID id, int io_state){
    HAL_StatusTypeDef rc = HAL_OK;
    uint8_t _io_state = 0x00;
    // Check if the TCA6408A is already initialized
    if( tca6408a_addr == 0){
    	  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
    	  HAL_Delay(10);  // Hold reset low for 10ms
    	  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
    	  HAL_Delay(10);  // Wait for the expander to come out of reset
        for (uint8_t addr = TCA6408A_ADDR_START; addr <= TCA6408A_ADDR_END; addr++)
        {
            // HAL_I2C_IsDeviceReady expects the 7-bit address shifted left.
            if (HAL_I2C_IsDeviceReady(&hi2c2, addr << 1, 3, 100) == HAL_OK)
            {
                tca6408a_addr = addr << 1;
                break;
            }
        }
        if (tca6408a_addr == 0)
        {
            printf("TCA6408A not found on I2C bus!\r\n");
            return -1;
        }else{
            printf("TCA6408A address is: 0x%02X\r\n", tca6408a_addr/2);
        }
    }
  
    // Configure the TCA6408A: Set P0 and P1 as outputs (bits = 0)
    // and the rest as inputs (bits = 1). 0xFC => 1111 1100.
    rc = HAL_I2C_Mem_Read(&hi2c2, tca6408a_addr, TCA6408A_INPUT_REG,
                            I2C_MEMADD_SIZE_8BIT, &_io_state, 1, HAL_MAX_DELAY);
    if (rc != HAL_OK)
    {
        printf("Failed to read TCA6408A input state %d\r\n", rc);
        return rc;
    }


    config = config &( ~(1<<id)); // Set the bit corresponding to the id to 1 (input)
    if (HAL_I2C_Mem_Write(&hi2c2, tca6408a_addr, TCA6408A_CONFIG_REG,
                            I2C_MEMADD_SIZE_8BIT, &config, 1, HAL_MAX_DELAY) != HAL_OK)
    {
        printf("Failed to configure TCA6408A\r\n");
        return -1;
    }
    printf("TCA6408A configured\r\n");
    
    output_state = (_io_state & (~(1<<id ))) | (io_state<<id);
    
    rc = HAL_I2C_Mem_Write(&hi2c2, tca6408a_addr, TCA6408A_OUTPUT_REG,
                        I2C_MEMADD_SIZE_8BIT, &output_state, 1, HAL_MAX_DELAY);
    if (rc != HAL_OK)
    {
      printf("Failed to initialize TCA6408A output %d\r\n", rc);
    }
    return rc;
}
